#include "features.h"
#include "functions.h"
#include "load.h"
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/pfh.h>
#include <pcl/features/vfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/filter.h>
#include <math.h>
#include <utility>



void removeNaNFromFeatures (PointCloud<FPFHSignature33>::Ptr features)
{
    PointCloud<FPFHSignature33>::Ptr filteredFeatures (new PointCloud<FPFHSignature33>);
    bool isNaN=false;
    for (int i=0;i<features->size();i++)
    {
        for (int j=0;j<33;j++)//scorre tutta la feature
            if (pcl_isnan (features->at(i).histogram[1]))
            {//se trova un NaN imposta il flag true..
                isNaN=true;
                break;
            }
        if (!isNaN)
            filteredFeatures->push_back(features->at(i));
    }
    *features=*filteredFeatures;
}

void fpfhWithUniformKeypoints(
        PointCloud<PointXYZI>::Ptr keypoints,
        PointCloud<FPFHSignature33>::Ptr features,
        double radius)
{
    //cout << "FPFH Descriptors extraction... " << flush;
    features->clear();
    PointCloud<PointXYZ>::Ptr kpts(new PointCloud<PointXYZ>);
    copyPointCloud(*keypoints, *kpts);//passo da XYZI a XYZ
    PointCloud<Normal>::Ptr normals (new  PointCloud<Normal>);
    onlyNormals<PointXYZ>(*kpts,*normals,radius*0.8);//20
    FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33>::Ptr feature_extractor (new FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33>);
    feature_extractor->setSearchMethod (search::Search<PointXYZ>::Ptr (new search::KdTree<PointXYZ>));
    feature_extractor->setRadiusSearch (radius);//25
    feature_extractor->setInputNormals(normals);
    feature_extractor->setInputCloud(kpts);
    feature_extractor->compute (*features);
    removeNaNFromFeatures(features);
}
